#!/bin/bash

# 确保脚本有执行权限
# chmod +x create_best_kitti_map.sh

# 设置ROS环境
source /root/catkin_ws/devel/setup.bash

# 定义颜色
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
NC='\033[0m' # No Color

# 定义变量
MAPS_DIR="/root/catkin_ws/src/auto_navigation/maps"
FINAL_MAP_NAME="kitti_best_map.pcd"
# 设置建图时间（秒）
MAPPING_DURATION=300

# 创建地图目录（如果不存在）
mkdir -p "$MAPS_DIR"

echo -e "${GREEN}=== 开始KITTI数据集高质量NDT建图流程 ===${NC}"
echo "地图将自动保存到时间戳目录: $MAPS_DIR/YYYYMMDD_HHMMSS/"
echo "最新地图目录将链接为: $MAPS_DIR/latest_mapping/"

# 步骤1: 运行优化的NDT建图
echo -e "${YELLOW}步骤1: 运行优化的NDT建图...${NC}"
echo "注意: 建图过程将运行 ${MAPPING_DURATION} 秒后自动停止。"
echo "开始建图..."

# 运行优化的NDT建图，在后台运行
roslaunch auto_navigation kitti_ndt_mapping_optimized.launch auto_save_with_timestamp:=true auto_save_interval:=30.0 &
MAPPING_PID=$!

# 等待指定的时间
echo "建图进程正在运行，将在 ${MAPPING_DURATION} 秒后自动停止..."
echo "期间每30秒会自动保存一次地图..."
sleep $MAPPING_DURATION

# 终止建图进程
echo "建图时间已到，正在停止建图进程..."
kill $MAPPING_PID
sleep 5  # 给进程一些时间来保存最终地图

# 步骤2: 优化最新地图
echo -e "${YELLOW}步骤2: 优化最新地图...${NC}"
LATEST_DIR="$MAPS_DIR/latest_mapping"
LATEST_MAP="$LATEST_DIR/kitti_latest_map.pcd"
if [ -f "$LATEST_MAP" ]; then
    OPTIMIZED_MAP="${LATEST_MAP%.*}_optimized.pcd"
    /root/catkin_ws/src/auto_navigation/scripts/optimize_map.sh "$LATEST_MAP" "$OPTIMIZED_MAP" "auto"
    
    # 步骤3: 创建最终地图的符号链接
    echo -e "${YELLOW}步骤3: 创建最终地图的符号链接...${NC}"
    ln -sf "$OPTIMIZED_MAP" "$LATEST_DIR/$FINAL_MAP_NAME"
    ln -sf "$OPTIMIZED_MAP" "$MAPS_DIR/$FINAL_MAP_NAME"
    echo "符号链接已创建: $LATEST_DIR/$FINAL_MAP_NAME -> $OPTIMIZED_MAP"
    echo "符号链接已创建: $MAPS_DIR/$FINAL_MAP_NAME -> $OPTIMIZED_MAP"
else
    echo "错误: 未找到最新地图文件!"
    exit 1
fi

echo -e "${GREEN}=== KITTI数据集高质量NDT建图流程完成 ===${NC}"
echo "最终地图: $MAPS_DIR/$FINAL_MAP_NAME"
echo "地图目录: $LATEST_DIR"
echo "是否查看最终地图? (y/n)"
read -t 10 VIEW_MAP  # 10秒超时
if [ "$VIEW_MAP" = "y" ] || [ "$VIEW_MAP" = "Y" ]; then
    /root/catkin_ws/src/auto_navigation/scripts/view_map.sh "$MAPS_DIR/$FINAL_MAP_NAME"
fi 